import functions as f
import robo3pi as s
import time
def moveRobot(INPUT_PATH, SOURCE_FILE_PATH, WORKSPACE_SIZE, SPEED):
    
    # Open source file to extract source point
    file = open(SOURCE_FILE_PATH)
    row = file.readline() # read first line
    row = file.readline().split() # read second line where we have the source point
    lastPoint = [float(row[0]), float(row[1])]  # get source point coordinates
    
    
    # Open path file
    file = open(INPUT_PATH)
    
    for line in file.read().split('\n'):
        print("Parsing the line: " + line)
        lineParts = line.split()
        if (lineParts[0] == "id"):
            robotId = "2"
            print("Robot ID set to: " + robotId)
        elif (lineParts[0] == "angle"):
            angle = int(float(lineParts[1]))
            print("Rotating robot " + robotId + " in angle: " + str(angle))
            f.rotateByAngle(robotId, angle) # Tell the robot to rotate in 'angle' degress (Can be positive for right turn and negative for left turn)
        elif (lineParts[0] == "line"):
            lineX = float(lineParts[1])
            lineY = float(lineParts[2])
            length = pow(abs((pow((lineX - lastPoint[0]) , 2)) - (pow((lineY - lastPoint[1]) , 2))), 0.5) *  (WORKSPACE_SIZE/2)
            lastPoint = [lineX, lineY]
            f.moveInLine(robotId, length, SPEED) # Tell the robot to move in a straight line of length 'length' and do it in 'movingTime' seconds
            time.sleep(0.2)
    file.close() # Close file
